clc; clear; close all;
% 对照vivado project QPSK_IMPLEMENTATION_USING_AD9361进行定点化验证
%% 发射机部分
fs = 10e6; %sample frequency
ps = 2.5e6; %symbol rate
M = 4; %MPSK
samPersymb = fs / ps; %samples per symbol

% 映射mapping
data = [];
% msg = [0, 1, 1, 1, 0, 1, 1, 1, 0, 1, 1, 1, 0, 0, 0, 0, 1, 0, 1, 0, 0];
% msg = repmat(msg, 1, 20);
load('data_to_be_send_fpga.mat');
msg = [0, 0, repmat(msg, 1, 10)];
% msg = msg(1:300);
for i = 1:2:length(msg) - 1

    if (msg(i) == 0 && msg(i + 1) == 0) data = [data, 1];
    elseif (msg(i) == 0 && msg(i + 1) == 1) data = [data, sqrt(-1)];
    elseif (msg(i) == 1 && msg(i + 1) == 0) data = [data, -1];
    elseif (msg(i) == 1 && msg(i + 1) == 1) data = [data, -1 * sqrt(-1)];
    end

end

% 4倍过采样并成型滤波
rollof = 0.5; %roll-off factor
rcos_fir = 1.15 * rcosdesign(rollof, 6, fs / ps, 'sqrt');
data_rcos = upfirdn(data, rcos_fir, samPersymb);
% 使用Farrow内插实现6倍内插
data_farrow = farrow_int(fs, 60e6, data_rcos);
signal_carrier = exp(sqrt(-1) * 2 * pi * (0:length(data_farrow) - 1) / 60e6 * 1000);
signal_rx = awgn(signal_carrier .* data_farrow, 10, 'measured');
signal_rx = signal_rx / max(abs(signal_rx));

%% 接收机部分
signal_rx = signal_rx .* exp(-1 * sqrt(-1) * 2 * pi * (0:length(signal_rx) - 1) / 60e6 * 1000);
rx_downsample = downsample(signal_rx, 6);
rx_rcos = filter(rcos_fir, 1, rx_downsample);
% debug用
figure; plot(real(rx_downsample), 'rs-');
figure; plot(real(rx_rcos), 'rs-');

%% gardner
index_sample = 1;
index_judge = 1;
index_symbol = 1;

c1 = 7.8e-3;
c2 = 3.82e-6;

time_error = 0;
w = 0.5;
nco = 0.7;
nco_temp = nco(1);
u = 0.6;
strobe = zeros(1, length(rx_rcos));
y = []';

temp1 = [];

while (index_sample < length(rx_rcos) - 2)
    nco_temp(index_sample + 1) = nco(index_sample) - w(index_symbol);

    if (nco_temp(index_sample + 1) > 0)
        nco(index_sample + 1) = nco_temp(index_sample + 1);
    else
        nco(index_sample + 1) = mod(nco_temp(index_sample + 1), 1);
        % Farrow内插
        FI1 = 0.5 * rx_rcos(index_sample + 2) - 0.5 * rx_rcos(index_sample + 1) ...
        -0.5 * rx_rcos(index_sample) + 0.5 * rx_rcos(index_sample - 1);

        FI2 = rx_rcos(index_sample + 1) + 0.5 * rx_rcos(index_sample + 1) ...
            -0.5 * rx_rcos(index_sample + 2) -0.5 * rx_rcos(index_sample) - 0.5 * rx_rcos(index_sample - 1);

        FI3 = rx_rcos(index_sample);
        y(index_judge) = FI1 * u(index_judge) * u(index_judge) + FI2 * u(index_judge) + FI3;

        strobe(index_judge) = mod(index_judge, 2);
        %当完成了strobe点的内插后，更新小数延时
        if (strobe(index_judge) == 0)
            %根据3个判决点计算时钟误差
            if (index_judge >= 3)
                time_error(index_symbol) = real(y(index_judge - 1)) * (real(y(index_judge)) - real(y(index_judge - 2))) ...
                    + imag(y(index_judge - 1)) * (imag(y(index_judge)) - imag(y(index_judge - 2)));
            else
                time_error(index_symbol) = real(y(index_judge - 1)) * real(y(index_judge)) ...
                    + imag(y(index_judge - 1)) * imag(y(index_judge));
            end

            %根据时钟误差更新环路滤波器的值
            if (index_symbol >= 2)
                temp1 = [temp1,time_error(index_symbol) - time_error(index_symbol - 1)];
                w(index_symbol + 1) = w(index_symbol) + c1 * (time_error(index_symbol) - time_error(index_symbol - 1)) + c2 * time_error(index_symbol);
            else
                w(index_symbol + 1) = w(index_symbol) + c1 * time_error(index_symbol) + c2 * time_error(index_symbol);
            end

            index_symbol = index_symbol + 1;
        end

        index_judge = index_judge + 1;
        u(index_judge) = nco(index_sample) /w(index_symbol);
    end

    index_sample = index_sample + 1;
end

close all;
subplot(211); plot((0:length(rx_rcos) - 1) / (10e6), real(rx_rcos)); hold on
subplot(211); plot((0:length(y) - 1) / (5e6), real(y), 'rs');
subplot(211); plot((0:length(y(2:2:end)) - 1) / (2.5e6), real(y(2:2:end)), 'b*');
hold off;
subplot(212); plot(time_error); title(['加入环路滤波，c1=', num2str(c1), ',c2=', num2str(c2)]);
